
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.templates.assets.shooterstates.ZingStateMachine;
import edu.wpi.first.wpilibj.templates.commands.Shooter;
import edu.wpi.first.wpilibj.templates.commands.AnalogSensorTest;
import edu.wpi.first.wpilibj.templates.commands.BumpArm;
import edu.wpi.first.wpilibj.templates.commands.ChangeIndexerState;
import edu.wpi.first.wpilibj.templates.commands.DigitalScale;
import edu.wpi.first.wpilibj.templates.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.commands.Ingest;
import edu.wpi.first.wpilibj.templates.commands.SetArmPosition;
import edu.wpi.first.wpilibj.templates.commands.StopShooter;
import edu.wpi.first.wpilibj.templates.commands.Track;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem.PresetArmPosition;
import edu.wpi.first.wpilibj.templates.subsystems.ShooterSubsystem;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    
    /*
     * Global Class Variables
     */
    private LogitechController driverController;
    private LogitechController coDriverController;
    
    private LogitechControllerButton driverA;
    private LogitechControllerButton driverB;
    private LogitechControllerButton driverX;
    private LogitechControllerButton driverY;
    private LogitechControllerButton driverLeftBumper;
    private LogitechControllerButton driverLeftTrigger;
    private LogitechControllerButton driverRightBumper;
    private LogitechControllerButton driverRightTrigger;
    private LogitechControllerButton driverStart;
    private LogitechControllerButton driverBack;
    
    private LogitechControllerButton coDriverA;
    private LogitechControllerButton coDriverB;
    private LogitechControllerButton coDriverX;
    private LogitechControllerButton coDriverY;
    private LogitechControllerButton coDriverLeftBumper;
    private LogitechControllerButton coDriverLeftTrigger;
    private LogitechControllerButton coDriverRightBumper;
    private LogitechControllerButton coDriverRightTrigger;
    private LogitechControllerButton coDriverStart;
    private LogitechControllerButton coDriverBack;
    
    // CONSTRUCTOR //
    
    
    public OI() {
        
        driverController = new LogitechController(1);
            driverA = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonA.value);
            driverB = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonB.value);
            driverX = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonX.value);
            driverY = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonY.value);
            driverLeftBumper = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonLeftB.value);
            driverLeftTrigger = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonLeftTrigger.value);
            driverRightBumper = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonRightB.value);
            driverRightTrigger = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonRightTrigger.value);
            driverStart = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonStart.value);
            driverBack = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonBack.value);            
        
            driverLeftBumper.whenPressed(new DigitalScale(DigitalScale.HALF_SCALE));
            driverLeftTrigger.whenPressed(new DigitalScale(DigitalScale.FULL_SCALE));
            
        coDriverController = new LogitechController(2);
            coDriverA = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonA.value);
            coDriverB = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonB.value);
            coDriverX = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonX.value);
            coDriverY = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonY.value);
            coDriverLeftBumper = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonLeftB.value);
            coDriverLeftTrigger = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonLeftTrigger.value);
            coDriverRightBumper = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonRightB.value);
            coDriverRightTrigger = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonRightTrigger.value);
            coDriverStart = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonStart.value);
            coDriverBack = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonBack.value);
            
            coDriverA.whenPressed(new SetArmPosition(PresetArmPosition.ON_GROUND));
            coDriverB.whenPressed(new SetArmPosition(PresetArmPosition.THRITY_DEGREES));
            coDriverX.whenPressed(new SetArmPosition(PresetArmPosition.ABOVE_THIRTY));
            coDriverY.whenPressed(new SetArmPosition(PresetArmPosition.MAIL_BOX_POSITION));
            
            coDriverLeftBumper.whenPressed(new ChangeIndexerState(ZingStateMachine.SPIT_OUT));
            coDriverLeftTrigger.whenPressed(new ChangeIndexerState(ZingStateMachine.INGEST));
     
            coDriverRightBumper.whenPressed(new Track());
            coDriverRightBumper.whenReleased(new StopShooter());
            coDriverRightTrigger.whenPressed(new ChangeIndexerState(ZingStateMachine.CLEAR_SHOOTER));
            
            coDriverStart.whenPressed(new BumpArm(25));
            coDriverBack.whenPressed(new BumpArm(-25));
            

    } // end OI
    
    /**
     * Gets the Driver's controller for other files to interface with.
     * 
     * @return the Driver's controller.
     */
    
    public LogitechController getDriverController() {
        return driverController;
    } // end getDriverController
    
    /**
     * Gets the Co-Driver's controller for other files to interface with.
     * 
     * @return The Co-Driver's controller.
     */
    
    public LogitechController getCoDriverController() {
        return coDriverController;
    } // end getCoDriverController
    
} // end IO

